// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and 
// the qglviewer library : http://www.libqglviewer.com


#ifndef  _OpenMeca_Geom_FRAME_hpp_
#define  _OpenMeca_Geom_FRAME_hpp_

#include <iostream>
#include <cassert>

#include <GL/gl.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include "OpenMeca/Geom/SpaceDim.hpp"

using OpenMeca::Geom::_3D;
using OpenMeca::Geom::SpaceDim;

namespace OpenMeca
{
  namespace Geom
  {
    
    template<SpaceDim N> class Point;
    template<SpaceDim N> class Quaternion;
    template<SpaceDim N> class Vector;

    template<SpaceDim N>
    class Frame
    {

    public:
      static const Frame<N> Global;
      static const Point<N> GlobalCenter;
      static const Quaternion<N> GlobalQuaternion;
      static const Frame<N> Null;
      static const Point<N> NullCenter;
      static const Quaternion<N> NullQuaternion;
      static std::string GetStrKey(){return std::string("Frame" + SpaceDimUtil<N>::GetStrKey());}

      static const Frame<N>& GetGlobal() {return Global;}
   
    public:
      //CONSTRUCTORS & DESTRUCTORS
      Frame(const Frame& frame);
      Frame(const Point<N> &center, const Quaternion<N> &quaternion);
      Frame();
      ~Frame();

      //OPERATORS
      Frame& operator=(const Frame& frame);

      const Quaternion<N> & GetQuaternion() const;
      const Point<N> & GetCenter() const;
      const Vector<N>& GetXAxis() const;
      const Vector<N>& GetYAxis() const;
      const Vector<N>& GetZAxis() const;
      SpaceDim GetDimension() const ;
      const Frame<N>& GetReferenceFrame() const;

      //USEFULL
      const Frame<N>* GetCommonFrame(Frame<N> *f) const;
      int GetRank() const { return rank_; };


      const GLdouble* GetGLMatrix() const {return &m_[0][0];}

      void UpdateGLMatrix();
      void UpdateGLMatrix() const; 

      const Frame<_3D>& GetMe() const;

    private:
      // - BOOST SERIALIZATION - //
      friend class boost::serialization::access;
      template<class Archive> void save(Archive&, const unsigned int) const;
      template<class Archive> void load(Archive&, const unsigned int); 
      BOOST_SERIALIZATION_SPLIT_MEMBER()     
      
    private:
      const SpaceDim dimension_;
      const Point<N>* const p_;
      const Quaternion<N>* const q_;
      const int rank_;	       // rank_ = 0 is for the Global Frame
#ifndef SERVER
      GLdouble m_[4][4];
#endif
      const Vector<N>* xAxis_; // Needs pointer because of multiple inclusion of .hpp  :(
      const Vector<N>* yAxis_; // Needs pointer because of multiple inclusion of .hpp  :(
      const Vector<N>* zAxis_; // Needs pointer because of multiple inclusion of .hpp  :(      

    private:
      void BuildAxis();
      void DeleteAxis();
    };

    //*************************
    //DECLARATION OF EXTERN OPERATORS
      
    template<SpaceDim N>
    std::ostream& operator<< (std::ostream& o, const Frame<N>& f);

    template<SpaceDim N>
    bool operator==(const Frame<N>& f1, const Frame<N>& f2);
    
    template<SpaceDim N>
    bool operator!=(const Frame<N>& f1, const Frame<N>& f2);
    
    
    template<SpaceDim N>
    std::ostream& operator<< (std::ostream& o, const Frame<N>& f);
    
    
    //
    // Vorbidden non-specialized methods (sould be specialized)
    //

    //
    // Template methods
    //

    // Constructors ...

    template<SpaceDim N> inline
    Frame<N>::Frame()
      : dimension_(N), 
	p_(0), 
	q_(0), 
	rank_(-1)
    {
      BuildAxis();
    }

    template<SpaceDim N> inline
    Frame<N>::Frame(const Point<N>& center, const Quaternion<N>& quaternion)
      : dimension_(N), 
	p_(&center), 
	q_(&quaternion), 
	rank_( (this ==  &Frame<N>::Global) ? 0 : center.GetFrame().rank_+1)
    {
      assert(center.GetFrame() == quaternion.GetFrameFrom()); //must be expressed in the SAME Frame !
      BuildAxis();
      const_cast<Quaternion<N> &>(*q_).SetFrameTo(this);      // Sorry !!! ;-)
    }

    template<SpaceDim N> inline
    Frame<N>::~Frame()
    {
      DeleteAxis();
    }
    

    // - BOOST SERIALIZATION - //
    template<SpaceDim N> 
    template<class Archive>
    void 
    Frame<N>::save(Archive& ar, const unsigned int ) const
    {
      assert(rank_>=0);
      ar << BOOST_SERIALIZATION_NVP(rank_);
      ar << BOOST_SERIALIZATION_NVP(p_);
      ar << BOOST_SERIALIZATION_NVP(q_);
    }

    // - BOOST SERIALIZATION - //
    template<SpaceDim N> 
    template<class Archive>
    void 
    Frame<N>::load(Archive& ar, const unsigned int ) 
      {
	ar >> BOOST_SERIALIZATION_NVP(const_cast<int&>(rank_));
	ar >> BOOST_SERIALIZATION_NVP(const_cast<Point<_3D>*&>(p_));
	ar >> BOOST_SERIALIZATION_NVP(const_cast<Quaternion<_3D>*&>(q_));
	assert(rank_>=0);
	if (rank_>0)
	  {
	    if (&q_->GetFrameTo() == &Frame<N>::Null)
	      const_cast<Quaternion<N> &>(*q_).SetFrameTo(this);
	  }
	else 	  
	  delete this;
	UpdateGLMatrix();
      }


    // Accessors ...
    template<SpaceDim N> inline
    const Frame<_3D>& 
    Frame<N>::GetMe() const
    {
      return *this;
    }

    template<SpaceDim N> inline
    const Point<N>&
    Frame<N>::GetCenter() const
    {
      return *p_;
    }

    template<SpaceDim N> inline 
    const Quaternion<N>&
    Frame<N>::GetQuaternion() const
    {
      return *q_;
    }

    template<SpaceDim N>inline
    SpaceDim
    Frame<N>::GetDimension() const
    {
      return dimension_;
    }

    template<SpaceDim N>inline
    const Frame<N>& 
    Frame<N>::GetReferenceFrame() const
    {
      return p_->GetFrame();
    }

    // External Operators ... 
     
    template<SpaceDim N>inline 
    bool 
    operator==(const Frame<N>& f1, const Frame<N>& f2)
    {
      // TODO
      return &f1 == &f2;
    }

    template<SpaceDim N>inline 
    bool 
    operator!=(const Frame<N>& f1, const Frame<N>& f2)
    {
      return  ! (f1 == f2);
    }

    template<SpaceDim N> inline
    std::ostream&
    operator<< (std::ostream& o, const Frame<N>& f)
    {
      return o << f.GetCenter() << '\t' << f.GetQuaternion();
    }

    // Class Operators ...


    template<SpaceDim N> inline 
    const Vector<N>& 
    Frame<N>::GetXAxis() const
    {
      return *xAxis_;
    }

    template<SpaceDim N> inline 
    const Vector<N>& 
    Frame<N>::GetYAxis() const
    {
      return *yAxis_;
    }

    template<SpaceDim N> inline 
    const Vector<N>& 
    Frame<N>::GetZAxis() const
    {
      return *zAxis_;
    }
    
    //Usefull methods
    template<SpaceDim N> inline
    const Frame<N>* 
    Frame<N>::GetCommonFrame(Frame<N> *f) const
    {
      Frame<N> *my_RefFrame = &GetReferenceFrame();
      Frame<N> *f_RefFrame = f;
      do
	{
	  do
	    {
	      if (my_RefFrame==f_RefFrame)
		{
		  return my_RefFrame;
		}
	      my_RefFrame = my_RefFrame->GetReferenceFrame();
	    }while(my_RefFrame!=&Frame<N>::Global);
	  f_RefFrame = f_RefFrame->GetReferenceFrame();
	}while(f_RefFrame!=&Frame<N>::Global);
      assert(0);
    }

    // Template spesialization 3D_
   

  }
}


#endif
